#include "common_functions.h"

#include <cmath>

// Function to rotate a point around a given center
cv::Point2f rotatePoint(const cv::Point2f& point, const cv::Point2f& center, float angle) {
    // Convert angle from degrees to radians
    // float rad = angle * CV_PI / 180.0;
    float rad = angle;

    // Create rotation matrix
    cv::Matx22f rotation_matrix(std::cos(rad), -std::sin(rad),
                                std::sin(rad),  std::cos(rad));

    // Translate point to origin (relative to center)
    cv::Point2f translated_point = point;
    // cv::Point2f translated_point = point - center;

    // Apply rotation
    cv::Point2f rotated_point = rotation_matrix * translated_point;

    // Translate back to original position
    return rotated_point + center;
}


cv::Point2f rotateAndTranslatePoint(const cv::Point2f& point, const cv::Mat& rotation_matrix, const cv::Point2f& offset) {
    return cv::Point2f(
        rotation_matrix.at<float>(0, 0) * point.x + rotation_matrix.at<float>(0, 1) * point.y,
        rotation_matrix.at<float>(1, 0) * point.x + rotation_matrix.at<float>(1, 1) * point.y
    ) + offset;
}

std::vector<int> get_unique_indexes(const std::vector<cv::Point>& points) 
{
    auto cmp = [](const cv::Point& a, const cv::Point& b) {
        if (a.x != b.x) return a.x < b.x;
        return a.y < b.y;
    };
    std::set<cv::Point, decltype(cmp)> seen(cmp);
    std::vector<int> unique_indexes;

    for (size_t i = 0; i < points.size(); ++i) {
        if (seen.find(points[i]) == seen.end()) {
            seen.insert(points[i]);
            unique_indexes.push_back(i);
        }
    }
    return unique_indexes;
}


bool is_in_2m_range(const cv::Point& pt, const cv::Point& center, int  map_len) 
{
    return (pt.x >= center.x - map_len && pt.x <= center.x + map_len &&
            pt.y >= center.y - map_len && pt.y <= center.y + map_len);
}


cv::Point world2grids(const cv::Point3f& pt, float gird_size) 
{
    return cv::Point(
        static_cast<int>(std::round(pt.x / gird_size)),
        static_cast<int>(std::round(pt.y / gird_size))
        // static_cast<int>(std::round(pt.z / gird_size))
    );
}


// 返回两个vector<int>，分别是list1和list2中相同点的下标
std::pair<std::vector<int>, std::vector<int>> find_common_point_indices(const std::vector<cv::Point>& list1, const std::vector<cv::Point>& list2)
{
    std::vector<int> idx1, idx2;
    for (int i = 0; i < list1.size(); ++i) {
        for (int j = 0; j < list2.size(); ++j) {
            if (list1[i] == list2[j]) {
                idx1.push_back(i);
                idx2.push_back(j);
            }
        }
    }
    return {idx1, idx2};
}


std::vector<cv::Point> select_points_by_indices(const std::vector<cv::Point>& points,
                                                const std::vector<int>& indices)
{
    std::vector<cv::Point> result;
    for (int idx : indices) {
        if (idx >= 0 && idx < points.size()) {
            result.push_back(points[idx]);
        }
        // 如果要抛异常可以写 else throw std::out_of_range(...)
    }
    return result;
}


bool isValueInVector(const std::vector<int>& vec, int value) 
{
    for (int element : vec) {
        if (element == value) {
            return true;
        }
    }
    return false;
}


unsigned char updateProbability(unsigned char PO, float P_Z_given_O, float P_Z_given_not_O)
{
    float P_O = PO / 256.0f;

    float P_Z = P_Z_given_O * P_O + P_Z_given_not_O * (1.0f - P_O);
    float P_O_given_Z = (P_Z_given_O * P_O) / P_Z;

    int ans = int(P_O_given_Z * 256);

    return (ans > 255) ? 255 : ans;
}



bool calculateAngle(const cv::Point& p1, const cv::Point& p2, int vis_r, int vis_deg)
{
    // 计算坐标差
    int delta_x = p2.x - p1.x;
    int delta_y = p2.y - p1.y;
    
    // 计算径向距离 r
    double r = std::sqrt(delta_x * delta_x + delta_y * delta_y);
    
    // 计算角度 theta（弧度制）
    double theta_radian = std::atan2(delta_y, delta_x);
    
    // 将角度转换为角度制
    double theta_degree = theta_radian * 180.0 / CV_PI;

    // 条件判断
    if (r < vis_r && (theta_degree < vis_deg && theta_degree > -vis_deg)) {
        return true;
    }
    
    return false;
}



int pointToIndex(const cv::Point& pt, int width, int height) 
{
    return pt.x * height + pt.y;
}


bool is_in_2m_range(const cv::Point2f& center, const cv::Point2f& pt, int map_len) {
    // 定义上下左右的范围
    float left_bound = center.x - map_len;
    float right_bound = center.x + map_len;
    float top_bound = center.y + map_len;
    float bottom_bound = center.y - map_len;

    // 检查目标点是否在范围内
    return (pt.x >= left_bound && pt.x <= right_bound && 
            pt.y >= bottom_bound && pt.y <= top_bound);
}